#include <intrins.h>
#include "Reg52.h"
#include "smi.h"

struct smi {
	void (*clk)(int id);
	void (*set_hi)(int id);
	void (*set_lo)(int id);
};

#define	PORT_HI(io, p)		(io |=  (1 << p))
#define	PORT_LO(io, p)		(io &= ~(1 << p))
#define	SDA_IN(io, p)		(io & (1 << p) ? 1 : 0)

#define	MDIO0		P1
#define	SCL0_PIN	3
#define	SDA0_PIN 	4

#define	MDIO1		P1
#define	SCL1_PIN	6
#define	SDA1_PIN 	5

#define	MDIO2		P1
#define	SCL2_PIN	0
#define	SDA2_PIN 	7

#define	SCL_HI(i)		PORT_HI(MDIO##i, SCL##i##_PIN)
#define	SCL_LO(i)		PORT_LO(MDIO##i, SCL##i##_PIN)
#define	SDA_HI(i)		PORT_HI(MDIO##i, SDA##i##_PIN)
#define	SDA_LO(i)		PORT_LO(MDIO##i, SDA##i##_PIN)

#define	SCL0_HI			PORT_HI(MDIO0, SCL0_PIN)
#define	SCL0_LO			PORT_LO(MDIO0, SCL0_PIN)
#define	SDA0_HI			PORT_HI(MDIO0, SDA0_PIN)
#define	SDA0_LO			PORT_LO(MDIO0, SDA0_PIN)
#define	SDA0_INPUT		do {(P1M0 |=  (1 << SDA0_PIN));(P1M1 &= ~(1 << SDA0_PIN));} while (0)
#define	SDA0_OUTPUT		do {(P1M0 &= ~(1 << SDA0_PIN));(P1M1 &= ~(1 << SDA0_PIN));} while (0)
#define	SDA0_IN			SDA_IN(MDIO0, SDA0_PIN)

#define	SDA1_INPUT		do {(P1M0 |=  (1 << SDA1_PIN));(P1M1 &= ~(1 << SDA1_PIN));} while (0)
#define	SDA1_OUTPUT		do {(P1M0 &= ~(1 << SDA1_PIN));(P1M1 &= ~(1 << SDA1_PIN));} while (0)
#define	SDA1_IN			SDA_IN(MDIO1, SDA1_PIN)

#define	SCL2_HI			PORT_HI(P2, SCL2_PIN)
#define	SCL2_LO			PORT_LO(P2, SCL2_PIN)
#define	SDA2_HI			PORT_HI(P1, SDA2_PIN)
#define	SDA2_LO			PORT_LO(P1, SDA2_PIN)
#define	SDA2_INPUT		do {(P1M0 |=  (1 << SDA2_PIN));(P1M1 &= ~(1 << SDA2_PIN));} while (0)
#define	SDA2_OUTPUT		do {(P1M0 &= ~(1 << SDA2_PIN));(P1M1 &= ~(1 << SDA2_PIN));} while (0)
#define	SDA2_IN			SDA_IN(P1, SDA2_PIN)

static void smi_delay(void)
{
	int i,j;

	for (i = 0; i < 10; i++) {
		for (j = 0; j < 10; j++) {
			_nop_();
			_nop_();
		}
	}
}

static void smi0_clk(uint8_t id)
{
	/* just ignore warning... */
	id = 0;

	SCL_HI(0);
	smi_delay();

	SCL_LO(0);
	smi_delay();
}

static void smi0_set_hi(uint8_t id)
{
	SDA0_OUTPUT;
	smi_delay();

	SDA_HI(0);
	smi_delay();

	smi0_clk(id);
}

static void smi0_set_lo(uint8_t id)
{
	SDA0_OUTPUT;
	smi_delay();

	SDA_LO(0);
	smi_delay();

	smi0_clk(id);
}

static void smi1_clk(uint8_t id)
{
	id = 0;

	SCL_HI(1);
	smi_delay();

	SCL_LO(1);
	smi_delay();
}

static void smi1_set_hi(uint8_t id)
{
	SDA1_OUTPUT;
	smi_delay();

	SDA_HI(1);
	smi_delay();

	smi1_clk(id);
}

static void smi1_set_lo(uint8_t id)
{
	SDA1_OUTPUT;
	smi_delay();

	SDA_LO(1);
	smi_delay();

	smi1_clk(id);
}

static void smi2_clk(uint8_t id)
{
	id = 0;

	SCL2_HI;
	smi_delay();

	SCL2_LO;
	smi_delay();
}

static void smi2_set_hi(uint8_t id)
{
	SDA2_OUTPUT;
	smi_delay();

	SDA2_HI;
	smi_delay();

	smi2_clk(id);
}

static void smi2_set_lo(uint8_t id)
{
	SDA2_OUTPUT;
	smi_delay();

	SDA2_LO;
	smi_delay();

	smi2_clk(id);
}

struct smi smi_ops[] = {
	{
		smi0_clk,
		smi0_set_hi,
		smi0_set_lo,
	},
	{
		smi1_clk,
		smi1_set_hi,
		smi1_set_lo,
	},
	{
		smi2_clk,
		smi2_set_hi,
		smi2_set_lo,
	}
};

#define	smi_clk(id)	smi_ops[id].clk(id)
#define	smi_set_hi(id)	smi_ops[id].set_hi(id)
#define	smi_set_lo(id)	smi_ops[id].set_lo(id)

void smi_init(void)
{
	SCL0_LO;
	SDA0_OUTPUT;

	SCL_LO(1);
	SDA1_OUTPUT;

	SCL2_LO;
	SDA2_OUTPUT;
}

static void smi_start(uint8_t id, uint8_t phy, uint8_t reg, int rw)
{
	uint16_t v = 0;
	int i;

	/* Preamble */
	for (i = 0; i < 32; i++)
		smi_set_hi(id);

	/* Start bit */
	smi_set_lo(id);
	smi_set_hi(id);

	/* op code */
	if (rw) {
		smi_set_hi(id);
		smi_set_lo(id);
	}
	else {
		smi_set_lo(id);
		smi_set_hi(id);
	}

	/* phy */
	for (i = 0; i < 5; i++) {
		if ((phy << i) & 0x10)
			smi_set_hi(id);
		else
			smi_set_lo(id);
	}

	/* reg */
	for (i = 0; i < 5; i++) {
		if ((reg << i) & 0x10)
			smi_set_hi(id);
		else
			smi_set_lo(id);
	}

	/* turn around */
	smi_set_hi(id);
	smi_set_lo(id);
}

void smi_write(uint8_t id, uint8_t phy, uint8_t reg, uint16_t value)
{
	int i;

	if (id > 2)
		return;

	smi_start(id, phy, reg, 0);

	for (i = 15; i >= 0; i--) {
		if (value & (1 << i))
			smi_set_hi(id);
		else
			smi_set_lo(id);
	}
}

uint16_t smi_read(uint8_t id, uint8_t phy, uint8_t reg)
{
	uint16_t v = 0;
	int i;

	if (id > 2)
		return 0;

	smi_start(id, phy, reg, 1);

	if (id == 0)
		SDA0_INPUT;
	else if (id == 1)
		SDA1_INPUT;
	else
		SDA2_INPUT;

	for (i = 15; i >= 0; i--) {
		if (id == 0)
			v |= (SDA0_IN ? (1 << i) : 0);
		else if (id == 1)
			v |= (SDA1_IN ? (1 << i) : 0);
		else
			v |= (SDA2_IN ? (1 << i) : 0);

		smi_clk(id);
	}

	return v;
}
